
#include "g_sc7a20.h"
#include "math.h"
#include "sys_param.h"

G_SENSER_t g_senser_data;
u8 read_data[6];

/*
SC7A20向寄存器写一个字节
RAddr：寄存器地址
WData：数据
*/
void SC7A20_Write_Byte(u8 RAddr, u8 WData)
{
	Start();
	SendData(SC7A20_I2C_WADDR);
	RecvACK();
	SendData(RAddr);
	RecvACK();
	SendData(WData);
	RecvACK();
	Stop();
}

/*
SC7A20从寄存器读一个字节
RAddr：寄存器地址
RData：接收数据地址
*/
void SC7A20_Read_Byte(u8 RAddr, u8 *RData)
{
	Start();
	SendData(SC7A20_I2C_WADDR);
	RecvACK();
	SendData(RAddr);
	RecvACK();
	Start();
	SendData(SC7A20_I2C_RADDR);
	RecvACK();
	*RData = RecvData(0);
	Stop();
}

/*
SC7A20从寄存器读一个字节
RAddr：寄存器地址
RData：接收数据地址
*/
void SC7A20_Read_Bytes(u8 RAddr, u8 *RData, u8 len)
{
	int i;
	Start();
	SendData(SC7A20_I2C_WADDR);
	RecvACK();
	SendData(RAddr | 0x80); // 连续读写--最高位=1
	RecvACK();
	Start();
	SendData(SC7A20_I2C_RADDR);
	RecvACK();

	for (i = 0; i < len - 1; i++)
	{
		RData[i] = RecvData(1);	// len-1 个ACK
	}
	RData[i] = RecvData(0);  // 最后一个是NACK
	Stop();

}

/*
SC7A20获取加速度值
accd_lsb：加速度值低寄存器地址
accd_msb：加速度值高寄存器地址
返回值accd_f：加速度值，单位mg
*/
int SC7A20_Get_ACCD(u8 accd_lsb, u8 accd_msb)
{
	u8 accd_l, accd_m = 0;
	unsigned int temp;
	int accd;

	SC7A20_Read_Byte(accd_lsb, &accd_l);
	SC7A20_Read_Byte(accd_msb, &accd_m);
	temp &= 0x0000;
	temp |= accd_m;
	temp <<= 8;
	temp &= 0xff00;
	temp |= accd_l;

	temp >>= 4; // 只有高12位有效

	if (temp & 0x0800)
	{						  // 负数 补码==>原码
		temp = temp & 0x07ff; // 屏弊最高位
		temp = ~temp;
		temp = temp + 1;
		temp = temp & 0x07ff;
		temp = -temp; // 还原最高位
	}
	else
	{
		temp &= 0x0fff;
	}
	accd = temp * 1;
	return accd;
}

/******************************** 
 * 官方文档例程，数据拼接
SC7A20获取加速度值
accd_lsb：加速度值低寄存器地址
accd_msb：加速度值高寄存器地址
返回值accd_f：加速度值，单位mg

**************************************/
int SC7A20_Get_ACCD_offical_demo(u8 accd_lsb, u8 accd_msb)
{
	u8 accd_l, accd_m = 0;
	unsigned int temp;
	int accd;

	SC7A20_Read_Byte(accd_lsb, &accd_l);
	SC7A20_Read_Byte(accd_msb, &accd_m);

	temp = (signed short int) ((accd_m<< 8) | accd_l); //拼接数据
	accd = temp>>6; //取 10 位带符号整型数据，Y,Z 同理
	return accd;
}

/*
SC7A20获取加速度值
accd_lsb：加速度值低寄存器地址
accd_msb：加速度值高寄存器地址
返回值accd_f：加速度值，单位mg
*/
int16_t SC7A20_12bitComplement(uint8_t msb, uint8_t lsb)
{

	int16_t temp;
	temp = msb << 8 | lsb;
	temp = temp >> 4; // 只有高12位有效
	temp = temp & 0x0fff;
	if (temp & 0x0800) // 负数 补码==>原码
	{
		temp = temp & 0x07ff; // 屏弊最高位
		temp = ~temp;
		temp = temp + 1;
		temp = temp & 0x07ff;
		temp = -temp; // 还原最高位
	}
	return temp;
}

/*
SC7A20获取xyz轴加速度值
*/
void SC7A20_GetInfo(void)
{
	// 获取三轴数据
	int x=0, y=0, z = 0;
	unsigned char i = 1;
	for (i = 1; i > 0; i--)
	{
		x += SC7A20_Get_ACCD(0x28, 0x29);
		y += SC7A20_Get_ACCD(0x2A, 0x2B);
		z += SC7A20_Get_ACCD(0x2C, 0x2D);
	}
	x /= 1;
	y /= 1;
	z /= 1;

	g_senser_data.cur_x = x;
	g_senser_data.cur_y = y;
	g_senser_data.cur_z = z;


	// printf("x:%hd mg, y:%hd mg, z:%hd mg \r\n",x, y, z);
}
/*
	return:   0 表示ID正确
	         -1 表示ID出错 
*/
int gsensor_id_check(void)
{
	uint8_t id_data;
	static uint8_t err_cnt = 0;
	
	SC7A20_Read_Byte(SC7A20_WHO_AM_I_REG, &id_data);
	// id_data = 1;
	if(id_data == SC7A20_ID){
		err_cnt = 0;
		return 0;
	}
	printf("gsensor_ID [%d]=?=[%d]\r\n", id_data, SC7A20_ID);
	err_cnt ++;
	if(err_cnt < 5){
		return 0;
	}
	
	return -1;
}

/*

*/
int get_gsensor_flag(void)
{
	return g_senser_data.active_flag;
}

void g_sensor_read(void)
{
	
	uint16_t max_move_size = SC7A20_MIN_MOVE_SIZE;
	static uint16_t print_delay;

	if ((sys_data.sta == SLEEP_STA) || (sys_data.sta == CHARGE_STA)|| (sys_data.sta == CHARGE_FINISH_STA))
	{
		if (g_senser_data.active_flag)
		{
			g_senser_data.active_flag = 0;
		}

		return;
	}

#if 1
	SC7A20_GetInfo();	// 先进行一次分段读取，然后再进行连续读取
	g_senser_data.move_size = abs(g_senser_data.cur_x - g_senser_data.last_x) + abs(g_senser_data.cur_y - g_senser_data.last_y)\
								 + abs(g_senser_data.cur_z - g_senser_data.last_z);

#else
	 SC7A20_Read_Bytes(0x28, read_data, 6);
//	printf("x=[%0x:%0x] y=[%0x:%0x] z=[%0x:%0x] \r\n",read_data[0],read_data[1],
//	    read_data[2], read_data[3], read_data[4], read_data[5]);

	g_senser_data.cur_x = SC7A20_12bitComplement(read_data[1], read_data[0]);
	// printf("x:[%d] mg ", g_senser_data.cur_x);
	g_senser_data.cur_y = SC7A20_12bitComplement(read_data[3], read_data[2]);
	// printf("y:[%d] mg ", g_senser_data.cur_y);
	g_senser_data.cur_z = SC7A20_12bitComplement(read_data[5], read_data[4]);
	// printf("z:[%d] mg\r\n", g_senser_data.cur_z);

	g_senser_data.move_size = abs(g_senser_data.cur_x - g_senser_data.last_x) + abs(g_senser_data.cur_y - g_senser_data.last_y) + abs(g_senser_data.cur_z - g_senser_data.last_z);
#endif
	print_delay++;
	if(0 == print_delay%20){
#ifdef DEBUG_OPEN_PRINT
		printf("x:%hd mg, y:%hd mg, z:%hd mg \r\n",g_senser_data.cur_x, g_senser_data.cur_y, g_senser_data.cur_z);
 		printf("move_size:[%d]\r\n", g_senser_data.move_size);
#endif		
	}


	// printf("move_size:[%d]  MIN_MOVE_SIZE:[%d] \r\n", g_senser_data.move_size, SC7A20_MIN_MOVE_SIZE);
	// 身体模式 --更迟钝些才行，否则身体抽动容易触发
	if(BODY_MODE == get_sys_mode()){
		max_move_size = BODY_MODE_MIN_MOVE_SIZE;
	}
	if (g_senser_data.move_size > max_move_size)
	{
		g_senser_data.active_cnt++;
		g_senser_data.unact_cnt = 0;

		if (g_senser_data.active_cnt > 3){
			if (g_senser_data.active_flag == 0){
				g_senser_data.active_flag = 1;
#ifdef DEBUG_OPEN_PRINT				
				printf("g_senser_data.active_flag == 1 \r\n");
#endif				
			}
		}
		// printf("move_size11:[%d] \r\n", g_senser_data.move_size);
	}
	else
	{
		g_senser_data.unact_cnt++;
		if (g_senser_data.unact_cnt > 5)	// 移动过程中可能会出现少量的 位移较少的数据，连续出现50次后，将active计数清0
		{
			g_senser_data.active_cnt = 0;
		}
//		
		if (g_senser_data.unact_cnt > 8){
			// printf("move_size2:[%d] \r\n", g_senser_data.move_size);
			if (g_senser_data.active_flag == 1){
				g_senser_data.active_flag = 0;
#ifdef DEBUG_OPEN_PRINT				
				printf("g_senser_data.active_flag == 0 \r\n");
#endif				
			}
		}
	}

	g_senser_data.last_x = g_senser_data.cur_x;
	g_senser_data.last_y = g_senser_data.cur_y;
	g_senser_data.last_z = g_senser_data.cur_z;
	
}

/************************************* 
SC7A20初始化
***************************************/
void SC7A20_Init(void)
{
	  SC7A20_Write_Byte(0x20,0x77);  //400Hz+正常模式xyz使能
	//SC7A20_Write_Byte(0x20, 0x47); // 50Hz+正常模式xyz使能
	//	SC7A20_Write_Byte(0x21,0x88);  //高通滤波：正常模式,00+滤波使能
	// SC7A20_Write_Byte(0x23,0x08);  //读取完成再更新、2g+正常模式
	
	 SC7A20_Write_Byte(0x21, 0x00); // 读取完成再更新、2g+正常模式
	 SC7A20_Write_Byte(0x22, 0x00); // 禁止中断
	SC7A20_Write_Byte(0x23, 0x08); // 读取完成再更新、2g+正常模式
}
